#include "quaternion.h"
#include <math.h>


QUATERNION QUATERNION::identity = QUATERNION(0.0F,0.0F,0.0F,1.0F); 

float QUATERNION::length()
{
	return sqrtf(x * x + y * y + z * z + w * w); 
}

void QUATERNION::normalize(QUATERNION *q)
{
	const float length = q->length(); 

	if(length > 0.0f)
	{
		q->x /= length; 
		q->y /= length; 
		q->z /= length; 
		q->w /= length; 
	}
}

QUATERNION QUATERNION::normalOf(QUATERNION *q)
{
	const float length = q->length(); 
	return (length > 0.0f) ? QUATERNION(q->x / length, q->y / length, q->z / length, q->w / length) : QUATERNION::identity; 
}

void QUATERNION::conjugate(QUATERNION *q)
{
	q->x *= -1; 
	q->y *= -1; 
	q->z *= -1; 
}

QUATERNION QUATERNION::conjugateOf(QUATERNION *q)
{
	return QUATERNION(-q->x, -q->y, -q->z, q->w); 
}

QUATERNION QUATERNION::operator * (const QUATERNION &q) const
{
  const float _x = w*q.x + x*q.w + y*q.z - z*q.y; 
  const float _y = w*q.y - x*q.z + y*q.w + z*q.x; 
  const float _z = w*q.z + x*q.y - y*q.x + z*q.w; 
  const float _w = w*q.w - x*q.x - y*q.y - z*q.z; 
  
  return QUATERNION(_x,_y,_z,_w); 
}

QUATERNION QUATERNION::operator=(const QUATERNION &q)
{
	x = q.x; 
	y = q.y; 
	z = q.z; 
	w = q.w; 
	return *this; 
}

bool QUATERNION::operator==(const QUATERNION &q) const 
{
	if(x != q.x)  return false; 
	if(y != q.y)  return false; 
	if(z != q.z)  return false; 
	if(w != q.w)  return false; 
	return true; 
}

bool QUATERNION::operator!=(const QUATERNION &q) const
{
	if(x != q.x)  return true; 
	if(y != q.y)  return true; 
	if(z != q.z)  return true; 
	if(w != q.w)  return true; 
	return false; 
}

QUATERNION::operator	float * ()
{
	return (float *) this; 
}
QUATERNION::operator	float * () const
{
	return (float *) this; 
}

void QUATERNION::RotateVector(const float angle, const Vector3f& axis, Vector3f* pInOut)
{
	QUATERNION temp, quat_view, result; 

	temp.x = axis.x * sinf(angle/2); 
	temp.y = axis.y * sinf(angle/2); 
	temp.z = axis.z * sinf(angle/2); 
	temp.w = cosf(angle/2); 

	quat_view.x = pInOut->x; 
	quat_view.y = pInOut->y; 
	quat_view.z = pInOut->z; 
	quat_view.w = 0; 

	QUATERNION t = (temp * quat_view) ; 
	QUATERNION::conjugate(&temp); 
	result =  t * temp; 

	pInOut->x = result.x; 
	pInOut->y = result.y; 
	pInOut->z = result.z; 
}